#include <Servo.h> //调用Servo类
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

Servo right;  //创建一个舵机控制对象
Servo left; // 使用Servo类最多可以控制8个舵机
MPU6050 mpu;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
int pos = 0;    // 该变量用与存储舵机角度位置
int setpointr = 40;//...............................................
long sumerrorr;
double kp = 0.3,ki = 0.15,kd = 0.1;
int lasterrorr;
int inputr = ypr[2] * 90+150;
int outputr;
int nowerrorr;
int setpointl = 0;//...............................................
long sumerrorl;
int lasterrorl;
int inputl =ypr[2] * 90+360;
int outputl;
int nowerrorl;
 


void setup() 
{ 
  right.attach(9);  // 该舵机由arduino第九脚控制
  left.attach(8);
  
  Serial.begin(115200);//..............................
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.CalibrateAccel(6);
  mpu.CalibrateGyro(6);
  mpu.PrintActiveOffsets();
  mpu.setDMPEnabled(true);
} 

void loop() 
{ 
  right.write(0);     // 复位
  left.write(0);
  if (ypr[1] * 180<30&&ypr[1] * 180>-30)
  {
    right.write(ypr[2] * 90+150 / M_PI);               // 指定舵机转向的角度
    left.write(ypr[2] * 90+360/ M_PI);
    delay(60);  
  }
  else if (ypr[1] * 180>30)
  {
    right.write(outputr);               // 指定舵机转向的角度
    left.write(outputl);
    delay(150); 
    nowerrorr = setpointr - inputr;
    sumerrorr += nowerrorr; 
    outputr = kp*nowerrorr + ki*sumerrorr + kd*(lasterrorr - nowerrorr);
    lasterrorr = nowerrorr;
    inputr += outputr;
    nowerrorl = setpointl - inputl;//.....................
    sumerrorl += nowerrorl; 
    outputl = kp*nowerrorl + ki*sumerrorl + kd*(lasterrorl - nowerrorl);
    lasterrorl = nowerrorl;
    inputl += outputl;
  }
  else if (ypr[1] * 180<-30)
  {
    right.write(100);               // 指定舵机转向的角度
    left.write(60);
    delay(150); 
  }
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))//............................
  {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print(" \t 摇头");
    Serial.print(ypr[0] * 180 / M_PI);
    Serial.print(" \t 点头");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print(" \t 晃头");
    Serial.println(ypr[2] * 180 / M_PI);
  }
    nowerrorr = setpointr - inputr;
    sumerrorr += nowerrorr; 
    outputr = kp*nowerrorr + ki*sumerrorr + kd*(lasterrorr - nowerrorr);
    lasterrorr = nowerrorr;
    inputr += outputr;
    nowerrorl = setpointl - inputl;//.....................
    sumerrorl += nowerrorl; 
    outputl = kp*nowerrorl + ki*sumerrorl + kd*(lasterrorl - nowerrorl);
    lasterrorl = nowerrorl;
    inputl += outputl;
    
}
